/**
 * @file ebex_quaternion.c
 *
 * @date Sep 30, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <math.h>
#include <omp.h>
#include <stdbool.h>
#include <mkl_lapacke.h>

#include <log_util.h>
#include <memory_util.h>
#include <random_util.h>
#include <ebex_matrix.h>
#include <ebex_quaternion.h>
#include <ebex_statistics.h>

double ebex_slerp_tol = 0.99999;

/**
 * Generates a direction cosine matrix from a given quaternion
 * @param m_result DCM matrix in which to store the result
 * @param m_src Quaternion from which to calculate
 */
void ebex_matrix_from_quat( ebex_mat3x3_t *m_result, const ebex_quat_t *m_src )
{
	double xx = m_src->x * m_src->x;
	double yy = m_src->y * m_src->y;
	double zz = m_src->z * m_src->z;
	double ww = m_src->w * m_src->w;

	double xy = 2.0 * m_src->x * m_src->y;
	double xz = 2.0 * m_src->x * m_src->z;
	double yz = 2.0 * m_src->y * m_src->z;

	double wx = 2.0 * m_src->w * m_src->x;
	double wy = 2.0 * m_src->w * m_src->y;
	double wz = 2.0 * m_src->w * m_src->z;

	m_result->col[0].x = ww + xx - yy - zz;
	m_result->col[0].y = xy - wz;
	m_result->col[0].z = xz + wy;
	m_result->col[1].x = xy + wz;
	m_result->col[1].y = ww - xx + yy - zz;
	m_result->col[1].z = yz - wx;
	m_result->col[2].x = xz - wy;
	m_result->col[2].y = yz + wx;
	m_result->col[2].z = ww - xx - yy + zz;

}

/**
 * Generates a quaternion from a direction-cosine (rotation) matrix
 * @param m_result Pointer to the allocated quaternion that will receive the result
 * @param m_src Pointer to the DCM
 */
void ebex_quat_from_matrix( ebex_quat_t *m_result, const ebex_mat3x3_t *m_src )
{
    double scale;

    double xx = m_src->col[0].x;
    double yx = m_src->col[0].y;
    double zx = m_src->col[0].z;
    double xy = m_src->col[1].x;
    double yy = m_src->col[1].y;
    double zy = m_src->col[1].z;
    double xz = m_src->col[2].x;
    double yz = m_src->col[2].y;
    double zz = m_src->col[2].z;

    double trace = ( 1.0 + ( xx + yy ) + zz );

    if ( trace > DBL_TINY )
    {
    	scale = 2.0 * sqrt(trace);
    	m_result->x = (zy - yz) / scale;
    	m_result->y = (xz - zx) / scale;
    	m_result->z = (yx - xy) / scale;
    	m_result->w = sqrt(trace) / 2.0;
    	return;
    }

    if (xx > yy && xx > zz)
    {
    	scale = sqrt(1.0 + xx - yy - zz) * 2.0;
    	m_result->x = sqrt(1.0 + xx - yy - zz) / 2.0;
    	m_result->y = (yx + xy) / scale;
    	m_result->z = (xz + zx) / scale;
    	m_result->w = (zy - yz) / scale;
    }
    else if (yy > zz)
    {
    	scale = sqrt(1.0 + yy - xx - zz) * 2.0;
    	m_result->x = (yx + xy) / scale;
    	m_result->y = sqrt(1.0 + yy - xx - zz) / 2.0;
    	m_result->z = (zy + yz) / scale;
    	m_result->w = (xz - zx) / scale;
    }
    else
    {
    	scale = sqrt(1.0 + zz - xx - yy) * 2.0;
    	m_result->x = (xz + zx) / scale;
    	m_result->y = (zy + yz) / scale;
    	m_result->z = sqrt(1.0 + zz - xx - yy) / 2.0;
    	m_result->w = (yx - xy) / scale;
    }

}

/**
 * Returns a naive-ly weighted mean of multiple quaternions
 * @param m_dest
 * @param m_input
 * @param m_weights
 * @param m_estimate
 * @param m_tol
 * @param m_maxiter
 * @param m_num
 */
static void ebex_quat_interal_mean(ebex_quat_t *m_dest, const ebex_quat_t *m_input,
		const double *m_weights, const ebex_quat_t *m_estimate, double m_tol, int m_maxiter, size_t m_num)
{
	size_t i;
	ebex_quat_t best_est;
	ebex_quat_t perpendicular;
	double costheta;
	double sintheta;
	ebex_quat_t mean_est;
	ebex_quat_t prev_best;

	ebex_quat_t *scaled_quats = (ebex_quat_t *) e_malloc(m_num * sizeof(ebex_quat_t));

	ebex_quat_copy(&best_est, m_estimate);
	for (size_t iter = 0; iter < m_maxiter; ++iter)
	{
#		pragma omp parallel for \
			shared (m_input, scaled_quats) \
			private(i, costheta, sintheta, perpendicular) \
			schedule(static)
		for (i = 0; i < m_num; i++)
		{
			costheta = ebex_quat_dot(&m_input[i], &best_est);

			ebex_quat_scalarmul(&perpendicular, &best_est, costheta);
			ebex_quat_sub(&perpendicular, &m_input[i], &perpendicular);
			sintheta = ebex_quat_norm(&perpendicular);

			if (!isnormal(sintheta))
			{
				ebex_quat_from_scalar(&scaled_quats[i], 0.0);
			}
			else
			{
				double theta = atan2(sintheta, costheta);
				ebex_quat_scalarmul(&scaled_quats[i], &perpendicular, theta / sintheta);
			}
		}

		/// Get the mean of the resultant quaternions
		ebex_quat_weighted_sum(&mean_est, scaled_quats, m_weights, m_num);

		/// Rotate best_est toward mean_est, for the new estimate.
		ebex_quat_copy(&prev_best, &best_est);

		ebex_quat_rotate_4vec(&best_est, &best_est, &mean_est);

		ebex_quat_sub(&prev_best, &prev_best, &best_est);

		if (ebex_quat_get_max_abs(&prev_best) <= m_tol) break;

	}

	e_free(scaled_quats);
	ebex_quat_copy(m_dest, &best_est);
}

/**
 * Performs quaternion averaging based on "Spherical Averages and Applications to Spherical
 * Splines and Interpolation." ACM Transactions on Graphics 20 (2001) 95-126.
 * @param m_dest Pointer to the final, best-fit quaternion
 * @param m_input Vector of input quaternions to average
 * @param m_weights Weights for the input quaternions.  If NULL, uniform weighting is assumed
 * @param m_errtol Minimum improvement size to continue
 * @param m_maxiter Maximum number of iterations to consider
 * @param m_num Number of quaternions to average
 */
void ebex_quat_spherical_weighted_mean(ebex_quat_t *m_dest, const ebex_quat_t *m_input, const double *m_weights,
		double m_errtol, int m_maxiter, size_t m_num)
{
	ebex_quat_t estimate;
	ebex_quat_t *corrected_quaternions;
	size_t i, j;

	ebex_quat_from_scalar(&estimate, 0.0);

	for (i = 0; i < m_num; ++i)
	{
		estimate.x += fabs(m_input[i].x);
		estimate.y += fabs(m_input[i].y);
		estimate.z += fabs(m_input[i].z);
		estimate.w += fabs(m_input[i].w);
	}

	assert(!isnan(estimate.w));
	for (i = 1, j = 0; i < 3; i++)
	{
		if (estimate.element[i] >= estimate.element[j])
		{
			estimate.element[j] = 0.0;
			j = i;
		}
		else estimate.element[i] = 0.0;
	}
	estimate.element[j] = 1.0;

	/// Ensure that all quaternions are pointing in the same hemisphere
	corrected_quaternions = e_malloc(m_num * sizeof(ebex_quat_t));
#	pragma omp parallel for \
		shared (corrected_quaternions, m_input) \
		private(i, estimate) \
		schedule(static)
	for (i = 0; i < m_num; i++)
	{
		if (ebex_quat_dot(&estimate, &m_input[i]) >= 0.0)
			ebex_quat_copy(&corrected_quaternions[i], &m_input[i]);
		else
			ebex_quat_neg(&corrected_quaternions[i], &m_input[i]);

		assert(ISDBL_TINY(ebex_quat_norm(&corrected_quaternions[i]) - 1.0));
	}

	ebex_quat_spherical_weighted_mean_corrected(m_dest, corrected_quaternions, m_weights, m_errtol, m_maxiter, m_num);

	assert(!isnan(m_dest->w));

	// Free up resources.
	e_free(corrected_quaternions);

}

void ebex_quat_spherical_weighted_mean_corrected(ebex_quat_t *m_dest, const ebex_quat_t *m_corrected_quaternions, const double *m_weights,
		double m_errtol, int m_maxiter, size_t m_num)
{

	ebex_quat_weighted_sum(m_dest, m_corrected_quaternions, m_weights, m_num);
	ebex_quat_normalize(m_dest, m_dest);

	/// Get the weighted average of the normalized, corrected quaternions
	ebex_quat_interal_mean(m_dest, m_corrected_quaternions, m_weights, m_dest,
			m_errtol, m_maxiter, m_num);

	ebex_quat_normalize(m_dest, m_dest);

}

void ebex_quat_regularize(const ebex_quat_t *m_input, ebex_quat_t *m_output, size_t m_num, const double m_bandwidth, const random_sample_3d_t *m_kernel, const ebex_mat3x3_t *m_intensity )
{
	ebex_3vec_t temp1;
	ebex_3vec_t temp2;
	ebex_mat3x3_t temp_mat;
	ebex_quat_t delta;
	size_t i;
#	pragma omp parallel for \
	   shared(m_input, m_output, m_kernel, m_intensity) \
	   private(i, temp1, temp2, temp_mat, delta) \
	   schedule(static)
	for( i = 0; i < m_num; i++ )
	{
		ran_sample_3d(m_kernel, temp1.element);
		ebex_3mat_scalar_mul(&temp_mat, &m_intensity[i], m_bandwidth);

		ebex_3mat_sqrt(&temp_mat, &temp_mat);

		ebex_3mat_mul_v3(&temp2, &temp_mat, &temp1);

		ebex_quat_from_3vec(&delta, &temp2, 1.0);
		ebex_quat_normalize(&delta, &delta);

		ebex_quat_mul(&m_output[i], &delta, &m_input[i]);
	}
}




/**
 * This is a discrete time update for the quaternion step.
 * \Phi(\omega) = \exp\left(\frac{1}{2}\begin{bmatrix}-[\omega \cross] & \omega | -\omega^T & 0\end{bmatrix}\Delta t\right)
 * Expand the matrix exponential as
 * \Phi(\omega) = \begin{bmatrix} Z_k & \phi_k | -\phi_k^T & \cos(0.5 \norm{\omega_k}\Delta t) \end{bmatrix}
 * with Z_k \equiv \cos(0.5 \norm{\omega_k}\Delta t)\cdot\I_{3x3} - \left[\phi_k\cross\right]
 * and  \phi_k \equiv sin(0.5 \norm{\omega_k}\Delta t)\omega_k / \norm{\omega_k}
 * @param m_result
 * @param m_inital
 * @param m_omega
 * @param m_sec
 */
void ebex_quat_propagate(ebex_quat_t *m_result, ebex_quat_t *m_initial, ebex_3vec_t *m_omega, double m_sec)
{
	ebex_quat_t temp_quat;
	ebex_3vec_t scaled_omega;
	double speed = ebex_3vec_length(m_omega);
	double dist = speed * (0.5 * m_sec);
	double cos_dist;

#ifdef EBEX_PRECISE
	vdSinCos(1, &dist, &sin_dist, &cos_dist);
	ebex_3vec_scalar_mul(&scaled_omega, m_omega, sin_dist / speed);
	ebex_quat_copy(&temp_quat, m_initial);

	m_result->x = ebex_kahan_sum(4, (double[]){cos_dist * temp_quat.x, scaled_omega.z * temp_quat.y, -scaled_omega.y * temp_quat.z, scaled_omega.x * temp_quat.w}, 1);
	m_result->y = ebex_kahan_sum(4, (double[]){-scaled_omega.z * temp_quat.x, cos_dist * temp_quat.y, scaled_omega.x * temp_quat.z, scaled_omega.y * temp_quat.w}, 1);
	m_result->z = ebex_kahan_sum(4, (double[]){scaled_omega.y * temp_quat.x, -	scaled_omega.x * temp_quat.y, cos_dist * temp_quat.z, scaled_omega.z * temp_quat.w}, 1);
	m_result->w = ebex_kahan_sum(4, (double[]){-scaled_omega.x * temp_quat.x, -scaled_omega.y * temp_quat.y, -scaled_omega.z * temp_quat.z, cos_dist * temp_quat.w}, 1);
#else
	if ( dist <= DBL_TINY )
	{
		if (m_result != m_initial)
			ebex_quat_copy(m_result, m_initial);
		return;
	}

	cos_dist = cos(dist);
	ebex_3vec_scalar_mul(&scaled_omega, m_omega, sin(dist) / speed);

	ebex_quat_copy(&temp_quat, m_initial);

	m_result->x = (	      cos_dist * temp_quat.x +	scaled_omega.z * temp_quat.y - 	scaled_omega.y * temp_quat.z + 	scaled_omega.x * temp_quat.w	);
	m_result->y = (-scaled_omega.z * temp_quat.x + 	      cos_dist * temp_quat.y +	scaled_omega.x * temp_quat.z + 	scaled_omega.y * temp_quat.w	);
	m_result->z = ( scaled_omega.y * temp_quat.x -	scaled_omega.x * temp_quat.y + 	      cos_dist * temp_quat.z +	scaled_omega.z * temp_quat.w	);
	m_result->w = (-scaled_omega.x * temp_quat.x - 	scaled_omega.y * temp_quat.y - 	scaled_omega.z * temp_quat.z +        cos_dist * temp_quat.w	);
#endif
}

/* Construct quaternion from Euler angles (in radians). */
void ebex_quat_from_euler(ebex_quat_t *m_quat, ebex_3vec_t *m_euler, e_rotation_order m_order)
{
	const rotation_order_t *rotation = GET_ROT_ORDER(m_order);
	int i=rotation->axis[0];
	int j=rotation->axis[1];
	int k=rotation->axis[2];
	double angle[3];
	double sinangle[3];
	double cosangle[3];
	double cos0cos2, cos0sin2, sin0cos2, sin0sin2;

	angle[0] = m_euler->element[i] * 0.5;
	angle[1] = m_euler->element[j] * (rotation->parity ? -0.5 : 0.5);
	angle[2] = m_euler->element[k] * 0.5;

	vdSinCos(3, angle, sinangle, cosangle);

	cos0cos2 = cosangle[0]*cosangle[2];
	cos0sin2 = cosangle[0]*sinangle[2];
	sin0cos2 = sinangle[0]*cosangle[2];
	sin0sin2 = sinangle[0]*sinangle[2];

	m_quat->element[i] = cosangle[1]*sin0cos2 - sinangle[1]*cos0sin2;
	if (rotation->parity)
		m_quat->element[j] = -cosangle[1]*sin0sin2 - sinangle[1]*cos0cos2;
	else
		m_quat->element[j] =  cosangle[1]*sin0sin2 + sinangle[1]*cos0cos2;
	m_quat->element[k] = cosangle[1]*cos0sin2 - sinangle[1]*sin0cos2;

	m_quat->element[3] = cosangle[1]*cos0cos2 + sinangle[1]*sin0sin2;

}

void ebex_quat_slerp(ebex_quat_t *m_result, double m_percent, const ebex_quat_t *m_unitquat0, const ebex_quat_t *m_unitquat1)
{
	ebex_quat_t start, tmpq_0, tmpq_1;
	double inv_sin, scale0, scale1, cos_angle, angle;
	cos_angle = ebex_quat_dot(m_unitquat0, m_unitquat1);
	if (cos_angle < 0.0)
	{
		cos_angle = -cos_angle;
		ebex_quat_neg(&start, m_unitquat0);
	}
	else
	{
		ebex_quat_copy(&start, m_unitquat0);
	}
	if (cos_angle < ebex_slerp_tol)
	{
		angle = acos(cos_angle);
		inv_sin = (1.0 / sin(angle) );
		scale0 = (sin(((1.0 - m_percent) * angle )) * inv_sin );
		scale1 = (sin((m_percent * angle )) * inv_sin );
	}
	else
	{
		scale0 = (1.0 - m_percent );
		scale1 = m_percent;
	}
	ebex_quat_scalarmul(&tmpq_0, &start, scale0);
	ebex_quat_scalarmul(&tmpq_1, m_unitquat1, scale1);
	ebex_quat_add(m_result, &tmpq_0, &tmpq_1);
}

void ebex_quat_mmse(ebex_quat_t *m_result, const ebex_quat_t *m_input, const double *m_weight, size_t m_num)
{
	ebex_mat3x3_t Bk = {{{0.0}}};
	ebex_mat3x3_t Bk_t;

	ebex_mat3x3_t tmp_3mat;
	ebex_mat4x4_t K;
	ebex_quat_t retval;

	ebex_3vec_t z;
	double trace;


	for (size_t i = 0; i < m_num; i++)
	{
		ebex_matrix_from_quat(&tmp_3mat, &m_input[i]);
		ebex_3mat_scalar_mul(&tmp_3mat, &tmp_3mat, m_weight[i]);
		ebex_3mat_add(&Bk, &Bk, &tmp_3mat);
	}

	trace = ebex_3mat_trace(&Bk);
	ebex_3mat_transpose(&Bk_t, &Bk);
	ebex_3mat_make_identity(&tmp_3mat);
	ebex_3mat_scalar_mul(&tmp_3mat, &tmp_3mat, trace);

	z.element[0] = Bk.col[1].element[2] - Bk.col[2].element[1];
	z.element[0] = Bk.col[2].element[0] - Bk.col[0].element[2];
	z.element[0] = Bk.col[0].element[1] - Bk.col[1].element[0];

	ebex_3mat_add(&Bk, &Bk, &Bk_t);
	ebex_3mat_sub(&Bk, &Bk, &tmp_3mat);

	K.col[0].element[0] = Bk.col[0].element[0];
	K.col[0].element[1] = Bk.col[0].element[1];
	K.col[0].element[2] = Bk.col[0].element[2];
	K.col[0].element[3] = z.element[0];

	K.col[1].element[0] = Bk.col[1].element[0];
	K.col[1].element[1] = Bk.col[1].element[1];
	K.col[1].element[2] = Bk.col[1].element[2];
	K.col[1].element[3] = z.element[1];

	K.col[2].element[0] = Bk.col[2].element[0];
	K.col[2].element[1] = Bk.col[2].element[1];
	K.col[2].element[2] = Bk.col[2].element[2];
	K.col[2].element[3] = z.element[2];

	K.col[3].element[0] = z.element[0];
	K.col[3].element[1] = z.element[1];
	K.col[3].element[2] = z.element[2];
	K.col[3].element[3] = trace;

	ebex_mat_eigenv(&K, NULL, &K);

	retval.element[0] = K.col[3].element[0];
	retval.element[1] = K.col[3].element[1];
	retval.element[2] = K.col[3].element[2];
	retval.element[3] = K.col[3].element[3];

	ebex_quat_normalize(m_result, &retval);

}

void ebex_quat_generate_attitude_cov(ebex_mat3x3_t *m_output, ebex_quat_t *m_input, ebex_quat_t *m_mean, double *m_weights, size_t m_num)
{
	ebex_3vec_t *output_ypr;
	ebex_3vec_t mean;
	double cov[9];
	output_ypr = e_malloc(m_num * sizeof(ebex_3vec_t));

#	pragma omp parallel for shared(output_ypr) schedule(static)
	for (size_t i = 0; i < m_num; i++)
	{
		ebex_3vec_euler_from_quat(&output_ypr[i], rotation_order_XYZ, &m_input[i]);
	}
	ebex_3vec_euler_from_quat(&mean, rotation_order_XYZ, m_mean);

//	ebex_stats_covariance(m_num, 3, output_ypr->element, 3, 1, true, cov);
	ebex_stats_weighted_covariance(m_num, 3, output_ypr->element, mean.element, m_weights, EBEX_3VEC_STRIDE, 1, true, cov);


	m_output->col[0].x = cov[0];
	m_output->col[0].y = cov[1];
	m_output->col[0].z = cov[2];

	m_output->col[1].x = cov[3];
	m_output->col[1].y = cov[4];
	m_output->col[1].z = cov[5];

	m_output->col[2].x = cov[6];
	m_output->col[2].y = cov[7];
	m_output->col[2].z = cov[8];

	e_free(output_ypr);

}

/**
 * Computes the exponential (base e) of a quaternion.  Thus #m_dest = e^(#m_exp)
 * @param m_dest Pointer to the receiving quaternion
 * @param m_exp Pointer to the originating quaternion
 */
void ebex_quat_exp(ebex_quat_t *m_dest, ebex_quat_t *m_exp)
{
	ebex_3vec_t svec;
	ebex_3vec_t identity = {{1.0, 1.0, 1.0}};
	double len;
	double re_exp = exp(m_exp->w);

	ebex_3vec_make_from_elems(&svec, m_exp->x, m_exp->y, m_exp->z);
	len = ebex_3vec_length(&svec);

  if ( ISDBL_TINY(len))
    {
	  ebex_quat_from_elems(m_dest, 0.0, 0.0, 0.0, re_exp * cos(len));
    }
  else
    {
	  ebex_3vec_copy_sign_per_elem(&identity, &identity, &svec);
	  ebex_3vec_scalar_mul(&identity, &identity, sin(len));
	  ebex_quat_from_3vec(m_dest, &identity, cos(len));
	  ebex_quat_scalarmul(m_dest, m_dest, re_exp);
    }
}

/**
 * Computes the logarithm of a quaternion.  Thus #m_dest = log(#m_logquat)
 * @param m_dest Pointer to the receiving quaternion
 * @param m_logquat Pointer to the originating quaternion
 */
void ebex_quat_log(ebex_quat_t *m_dest, ebex_quat_t *m_logquat)
{
	ebex_3vec_t svec;
	ebex_3vec_t identity = {{1.0, 1.0, 1.0}};
	double len;

	ebex_3vec_make_from_elems(&svec, m_logquat->x, m_logquat->y, m_logquat->z);
	len = ebex_3vec_length(&svec);
	  if ( ISDBL_TINY(len))
	    {
		  ebex_quat_from_elems(m_dest, 0.0, 0.0, 0.0, log(ebex_quat_norm(m_logquat)));
	    }
	  else
	  {
		  ebex_3vec_copy_sign_per_elem(&svec, &identity, &svec);
		  ebex_3vec_scalar_mul(&svec, &svec, ebex_quat_arg(m_logquat));
		  ebex_quat_from_3vec(m_dest, &svec, log(ebex_quat_norm(m_logquat)));
	  }
}

/**
 * This routine is based on the article "General Construction of Time-Domain Filters for Orientation Data", IEEE Trans. Vis. Comp. Gr.
 * Vol 8, #2, 2002.  Modifications have been made to ensure that the start and end points remain intact.
 * @param m_dest Destination quaternion vector
 * @param m_src Source quaternion vector (can be the same as the destination)
 * @param m_window Window vector, giving weighting for sequential samples
 * @param m_num Number of quaternions in #m_src/#m_dest
 * @param m_windowsize Number of values in the window
 * @param m_iterations Number of iterations to perform on the quaternion stream
 * @param m_gain Exponential gain value
 */
void ebex_quat_smooth(ebex_quat_t *m_dest, size_t m_dest_step_size, ebex_quat_t *m_src, size_t m_src_step_size,
                      double *m_window, size_t m_num, int m_windowsize, size_t m_iterations, double m_gain)
{
	ebex_quat_t *logquat;
	ebex_quat_t *input;
	ebex_quat_t *output;
	ebex_quat_t quat_working;
	int mid_window = m_windowsize / 2;
	size_t trans_len = m_num - 1;

	output = e_malloc(m_num * sizeof(ebex_quat_t));
	logquat = e_malloc(trans_len * sizeof(ebex_quat_t));
	input = e_malloc(m_num * sizeof(ebex_quat_t));

	for (size_t i = 0; i < m_num; i++)
	{
		ebex_quat_copy(&input[i], &m_src[i * m_src_step_size]);
	}

	for (size_t iter = 0; iter < m_iterations; iter++)
	{
		for (int i = 0; i < trans_len; i++)
		{
			ebex_quat_error(&quat_working, &input[i], &input[i+1]);
			ebex_quat_log(&logquat[i], &quat_working);
		}

		for (int i = 0; i < m_num; i++)
		{
			ebex_quat_from_scalar(&quat_working, 0.0);
			for (int window_pos = 0; window_pos < m_windowsize; window_pos++)
			{
				ebex_quat_t scaled_log;
				int j = i - mid_window + window_pos;
				if (j < 0) j = (-j);
				if (j >= trans_len) j = ((2 * trans_len) - j - 1);
				ebex_quat_scalarmul(&scaled_log, &logquat[j], m_window[window_pos]);
				ebex_quat_add(&quat_working, &quat_working, &scaled_log);
			}
			ebex_quat_scalarmul(&quat_working, &quat_working, m_gain);
			ebex_quat_exp(&quat_working, &quat_working);
			ebex_quat_mul(&output[i], &input[i], &quat_working);

		}
		EBEX_SWAP(input, output);
	}

	for (int i = 0; i < m_num; i++) ebex_quat_normalize(&m_dest[i * m_dest_step_size], &input[i]);


	e_free(logquat);
	e_free(input);
	e_free(output);
}

void ebex_quat_randomize_population(ebex_quat_t *m_quat, random_sample_t *m_rnd, size_t m_num)
{
	ebex_3vec_t rnd_dir;
	ebex_quat_t rotation;

	for (size_t i = 0; i < m_num; i++)
	{
		ran_unit_sphere_md(rnd_dir.element, 1.0, NULL, 3);
		ebex_quat_from_rotation_axis(&rotation, ran_sample(m_rnd), &rnd_dir);
		ebex_quat_mul(&m_quat[i], &m_quat[i], &rotation);
	}
}
